/*
 * Broadcast.cpp
 *
 *  Created on: 08/ott/2013
 *      Author: alessandrob
 */
#include<iostream>
#include<arpa/inet.h>
#include<unistd.h>
#include<sys/socket.h>
#include<sys/types.h>
#include<stdio.h>
#include<string.h>
#include<stdlib.h>
#include<opencv2/opencv.hpp>
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
#include "Utility.h"
#include "brisk/brisk.h"
//#include "brisk.h"
#define PORT 5002
#define PORTB 5003
#define BUFLEN
#define PAYLOAD 1462
#define TEST 5

void broadcast(int clientsNumber,float overlap,int loads,int threshold,int blocks,int octave)
{
	struct list_nodes nodeList[clientsNumber+1];
	int sockfd,countClientsConnected=0,i,sockBroadcast;
	struct sockaddr_in broadcastAddress,client_address,server_address;
	int broadcastPermission=1,row,dataSent,ovl,optVal = 1;
	char sync[30];
	char ready[5];
	char command[35];
	float bandwidth;
	double det_time;
	vector<int> params = vector<int>(2);
	vector<KeyPoint> kp,distributedKp;
	Mat descriptors,distributedDescriptors;
	params[0]=0;
	unsigned int slen=sizeof(client_address);
	Mat original,broadcastImage,cameraImage;
	vector<sockaddr_in> clients;
	sync[0]='s';
	FILE* file = fopen("/home/ubuntu/testBroadcastRealeFrame.txt","a+");
	if(file == NULL)
		return;
	FILE* lostpackets = fopen("/home/ubuntu/lostpacketsRealeFrame.txt","a+");
	if(file == NULL)
		return;
	FILE* filebandwidth = fopen("/home/ubuntu/BandwidthBroadcastRealeFrame.txt","a+");
	if(file == NULL)
		return;

	//Setting up BRISK

	//system("sudo cpufreq-set -f 720MHz");
	Ptr<FeatureDetector> detector = new cv::BriskFeatureDetector(threshold,octave);
	Ptr<DescriptorExtractor> descriptorExtractor = new cv::BriskDescriptorExtractor();
	//system("sudo cpufreq-set -f 275MHz");

	//First time
	original = imread("/home/ubuntu/frame0.jpg",0);
	detector->detect(original,kp);
	descriptorExtractor->compute(original,kp,descriptors);

	for(int j=0;j<TEST;j++)
	{
		//cout<<"Struct size: "<<sizeof(packet[0])<<endl;
		cout<<"Setting up UDP server..."<<endl;
		//UDP Socket (used only for the connections)
		if ((sockfd=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))==-1)
			printErrorLog("Server socket creation error. ");
		memset(&server_address, 0, sizeof(server_address));
		server_address.sin_family = AF_INET;
		server_address.sin_addr.s_addr = htonl(INADDR_ANY);
		//Listening port
		server_address.sin_port = htons(PORT);
		//Binding on port: 5002
		if (bind(sockfd, (struct sockaddr *)&server_address, sizeof(server_address))==-1)
			printErrorLog("Wrong binding. The server is NOT running");
		cout<<"Correct binding. The server is working correctly, listening for connections on port: 5002."<< endl;
		//Waiting for the connections
		int CPUspeed;
		uchar endAck[3];
		endAck[0]='e';
		endAck[1]='n';
		endAck[2]='d';
		char* ip;
		char address;
		countClientsConnected=0;
		clients.clear();
    //Attesa connessione di tutti i client.
		while(countClientsConnected!=clientsNumber)
		{
			if (recvfrom(sockfd, &ready[0], sizeof(char)*5, 0, (struct sockaddr*)&client_address, &slen)!=sizeof(char)*5)
				printErrorLog("No message received");
			printf("Beaglebone connected: %s:%d\n",inet_ntoa(client_address.sin_addr),ntohs(client_address.sin_port));
			ip = inet_ntoa(client_address.sin_addr);
			address = ip[8];
			nodeList[countClientsConnected+1].address = atoi(&address);
			//CPUspeed = deserializeSpeed(ready);
			CPUspeed = (ready[1] << 24);
			CPUspeed = CPUspeed + (ready[2] << 16);
			CPUspeed = CPUspeed + (ready[3] << 8);
			CPUspeed = CPUspeed + (ready[4]);
			nodeList[countClientsConnected+1].CPUspeed = CPUspeed;
			cout<<"CPU speed: "<<nodeList[countClientsConnected+1].CPUspeed<<endl;
			clients.push_back(client_address);
			countClientsConnected++;
		}
      
		//Broadcast UDP socket (setting parameters)
		if((sockBroadcast=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))<0)
			cout<<"Broadcast socket correctly created"<<endl;
		if(setsockopt(sockBroadcast,SOL_SOCKET,SO_BROADCAST,&broadcastPermission,sizeof(broadcastPermission))<0)
			printErrorLog("Setting socket option error ");
		memset(&broadcastAddress, 0, sizeof(broadcastAddress));
		broadcastAddress.sin_family = AF_INET;
		broadcastAddress.sin_addr.s_addr = htonl(INADDR_BROADCAST);
		broadcastAddress.sin_port = htons(PORT);

    //Test della banda media (praticamente inutile siccome la banda in trasmissione
    //broadcast è limitata a 9Mbps.
		for(i=0;i<clientsNumber;i++)
		{
			//ipAddress = inet_ntoa(clients[i].sin_addr);
			cout<<"TCP bandwidth test: "<<inet_ntoa(clients[i].sin_addr)<<endl;
			strcpy(command,"iperf -c");
			strcat(command,inet_ntoa(clients[i].sin_addr));
			strcat(command," -t 10 >/home/ubuntu/iperfLog.txt");
			system(command);
			//Iperf log file parsing to get the real bandwidth
			bandwidth=getBandwidth(false);
			if(bandwidth==0)
				printErrorLog("Network performance report file doesn't exist");
			//nodeList[i+1].ipAddress=ipAddress;
			fprintf(filebandwidth,"%f ",bandwidth);
			nodeList[i+1].bandwidth=bandwidth*1000000;
			printf("Bandwidth: %fMbps\n",nodeList[i+1].bandwidth/1000000);
		}
      
		//Clock synchronization
		//Starting timer
		sync[0]=(clientsNumber >> 24);
		sync[1]=(clientsNumber >> 16);
		sync[2]=(clientsNumber >> 8);
		sync[3]=(clientsNumber);
    //Pacchetto di sincronizzazione (non mi ricordo perchè 30 byte, mi pare avessi problemi
    //inviando un solo byte in broadcast. I primi 4 byte rappresentano il numero di client
    //durante l'esecuzione. I restanti byte sono junk.  
		for(int i=4;i<30;i++)
			sync[i] = 0;
		if (sendto(sockBroadcast, sync, sizeof(char)*30, 0, (struct sockaddr *)&broadcastAddress, sizeof(broadcastAddress)) !=sizeof(char)*30)
			printErrorLog("Clock synchronization failed");
		det_time=(double)getTickCount();
		cout<<"Sync ACK sent broadcastly"<<endl;
      
		//Calcolo dei load
		computeLoadBroadcast(clientsNumber,nodeList,overlap);
      
		//Ordinamento dei nodi
		orderNodes(clientsNumber,nodeList);
		double load_time = ((double)getTickCount() - det_time)/getTickFrequency();
		cout<<"Compute loads and ordering time: T+"<<load_time*1000<<"ms"<<endl;
      
		//Splitting dell'immagine
		row = floor((original.rows*nodeList[0].load)/100.0);
		cout<<"row: "<<row<<endl;
		nodeList[0].width=original.cols;
		nodeList[0].height=row;
		cout<<"From 0 to "<<row<<endl;
		//Image processed by the camera
		ovl = (int)(original.rows*overlap)/100;
		cameraImage = Mat(original,Rect_<int>(0,0,original.cols,row+ovl));
		cout<<"From "<<row<<" to "<<original.rows<<endl;
		//Image processed by the clients
		broadcastImage=Mat(original,Rect_<int>(0,row,original.cols,original.rows-row));
		double split_time = ((double)getTickCount() - det_time)/getTickFrequency();
		cout<<"Splitting the entire image time: T+"<<split_time*1000<<"ms"<<endl;

		//Creating packets
		cout<<"Creating packets..."<<endl;
		int k=0,lastPacket=0;
    //Numero di blocchi con cui suddividere l'immagine. Esso è dipendente dalla dimensione
    //dell'immagine e dalla dimensione del payload a livello 2.  
		blocks = floor((broadcastImage.rows*broadcastImage.cols)/PAYLOAD)+1;
		broadcast_packet packet[blocks];
		cout<<"Optimal number of packets: "<<blocks<<endl;
		int bytesPerPacket = (int)(broadcastImage.cols*broadcastImage.rows)/blocks;
		//Last packet could have different number of bytes
		if(bytesPerPacket*blocks != broadcastImage.cols*broadcastImage.rows)
			lastPacket = broadcastImage.cols*broadcastImage.rows - bytesPerPacket*blocks;
		cout<<"Bytes per packet: "<<bytesPerPacket<<endl;
		cout<<"Last packet: "<<lastPacket<<endl;
		//uchar support[bytesPerPacket];
		int bound;
		if(lastPacket > 0)
			bound = blocks-1;
		else
			bound = blocks;
    //Storage temporaneo dei pacchetti in una struttura dati.
		for(int i=0;i<bound;i++)
		{
			packet[i].packetNumber = i+1;
			//normal packets
			packet[i].payloadDimension = bytesPerPacket;
			createHeader(packet,bytesPerPacket,i+1);
			for(int x=0;x<bytesPerPacket;x++,k++)
				packet[i].data[x+8] = broadcastImage.data[k];
		}
		//last packet
		if(lastPacket>0)
		{
			packet[blocks-1].packetNumber = blocks;
			packet[blocks-1].payloadDimension = lastPacket;
			cout<<"Payload: "<<packet[blocks-1].payloadDimension<<endl;
			cout<<"Packet number: "<<packet[blocks-1].packetNumber<<endl;
			createHeader(packet,lastPacket,blocks);
			for(int x=0;x<lastPacket;x++,k++)
				packet[blocks-1].data[x+8] = broadcastImage.data[k];
		}
		double creating_time = ((double)getTickCount() - det_time)/getTickFrequency();
		cout<<"Creating packets time: T+"<<creating_time*1000<<"ms"<<endl;
		cout<<"Bytes actually copied: "<<k<<endl;

		//SENDING PACKETS
		//Sending information packet
    //Pacchetto di informazione da inviare in broadcast. Contiene tutti i dati relativi ad ogni
    //client (dimensioni frazione di immagine, numero blocchi, ip e load).  
		uchar infoPacket[12+clientsNumber*8];
		infoPacket[0]=(broadcastImage.cols >> 24);
		infoPacket[1]=(broadcastImage.cols >> 16);
		infoPacket[2]=(broadcastImage.cols >> 8);
		infoPacket[3]=(broadcastImage.cols);
		infoPacket[4]=(broadcastImage.rows >> 24);
		infoPacket[5]=(broadcastImage.rows >> 16);
		infoPacket[6]=(broadcastImage.rows >> 8);
		infoPacket[7]=(broadcastImage.rows);
		infoPacket[8]=(blocks >> 24);
		infoPacket[9]=(blocks >> 16);
		infoPacket[10]=(blocks >> 8);
		infoPacket[11]=(blocks);
		//Adding loads and ip addresses
		int counter = 12;
		for(int i=0;i<clientsNumber;i++,counter=counter+8)
		{
			infoPacket[counter] = (nodeList[i+1].address >> 24);
			infoPacket[counter+1] = (nodeList[i+1].address >> 16);
			infoPacket[counter+2] = (nodeList[i+1].address >> 8);
			infoPacket[counter+3] = (nodeList[i+1].address);
			infoPacket[counter+4] = ((int)nodeList[i+1].load >> 24);
			infoPacket[counter+5] = ((int)nodeList[i+1].load >> 16);
			infoPacket[counter+6] = ((int)nodeList[i+1].load >> 8);
			infoPacket[counter+7] = ((int)nodeList[i+1].load);
		}
		if(sendto(sockBroadcast, infoPacket, sizeof(char)*(12 + clientsNumber*8), 0, (struct sockaddr *)&broadcastAddress, sizeof(broadcastAddress)) <0)
			printErrorLog("Information packet not sent");
		cout<<"Information packet sent"<<endl;

		//Invio di tutti i pacchetti dell'immagine in broadcast.
		for(int i=0;i<blocks;i++)
		{
			dataSent = sendto(sockBroadcast, &packet[i].data[0], packet[i].payloadDimension+8, 0, (struct sockaddr *)&broadcastAddress, sizeof(broadcastAddress));
			//cout<<dataSent<<endl;
			if(dataSent!=packet[i].payloadDimension+8)
				printErrorLog("Block not sent correctly");
		}
		dataSent = sendto(sockBroadcast, endAck, sizeof(char)*3, 0, (struct sockaddr *)&broadcastAddress, sizeof(broadcastAddress));
		if(dataSent!=sizeof(char)*3)
			printErrorLog("End ack not sent correctly");

		double fake_sending_time = ((double)getTickCount() - det_time)/getTickFrequency();
		cout<<"Fake sending time: T+"<<fake_sending_time*1000<<"ms"<<endl;
		close(sockfd);
		close(sockBroadcast);

		//opening a new TCP socket (for receiving data and sending lost packets)
		if ((sockfd=socket(AF_INET, SOCK_STREAM, 0))==-1)
			printErrorLog("Server socket creation error. ");
		cout<<"Server TCP socket created correctly."<<endl;
		memset((char *) &server_address, 0, sizeof(server_address));
		server_address.sin_family = AF_INET;
		server_address.sin_port = htons(PORT);
		server_address.sin_addr.s_addr = htonl(INADDR_ANY);
		if(setsockopt(sockfd,SOL_SOCKET,SO_REUSEADDR,&optVal,sizeof(optVal))<0)
			printErrorLog("Setting socket option error ");
		//Binding on port: 5002
		if (bind(sockfd, (struct sockaddr *)&server_address, sizeof(server_address))==-1)
			printErrorLog("Wrong binding. The server is NOT running");
		cout<<"TCP socket for recovering the image. Listening for connections on port: 5002."<< endl;
		listen(sockfd,clientsNumber);
		countClientsConnected=0;
		int s,ipAddress;
		clients.clear();
    //Attesa connessione di tutti i client (TCP).  
		while(countClientsConnected!=clientsNumber)
		{
			if((s = accept(sockfd,(struct sockaddr*)&client_address,&slen))==-1)
				printErrorLog("Invalid client socket");
			ip = inet_ntoa(client_address.sin_addr);
			address = ip[8];
			ipAddress = atoi(&address);
			for(int i = 0;i < clientsNumber;i++)
			{
				cout<<"Address: "<<address<<" Address nodelist: "<<nodeList[i+1].address<<endl;
				if(nodeList[i+1].address == ipAddress)
				{
					cout<<"Socket copied"<<endl;
					nodeList[i+1].socket = s;
				}
			}
			//Receiving the packets to re-send
			cout<<"Beaglebone: 10.42.0."<<address<<" is ready to recover the image"<<endl;
			clients.push_back(client_address);
			countClientsConnected++;
		}
		//ORDINARE I NODI IN BASE AI LOAD IN MODO DECRESCENTE E INIZIARE LA RICHIESTA
		//CON IL REINVIO DEI PACCHETTI NON RICEVUTI CORRETTAMENTE
		char packetsRequest = 's';
		int lostPackets,packetNumber,count;
		for(int i=0;i<clientsNumber;i++)
		{
			s = nodeList[i+1].socket;
			cout<<"Recovering image of: 10.42.0."<<nodeList[i+1].address<<endl;
			if(send(s,&packetsRequest,sizeof(char),0)!=sizeof(char))
				printErrorLog("Ack not sent correctly");
      //Ricezione del numero di pacchetti persi.      
			if(recv(s,&lostPackets,sizeof(int),0)!=sizeof(int))
				printErrorLog("Lost packets number not received correctly");
			cout<<"Lost packets: "<<lostPackets<<endl;
			fprintf(lostpackets,"%d ",lostPackets);
			uchar packetsLost[lostPackets*4];
			//Ricezione dei pacchetti persi
			if(recv(s,&packetsLost[0],sizeof(char)*4*lostPackets,0)<0)
				printErrorLog("Lost packets received correctly");
			count = 0;
			for(int k=0;k<lostPackets;k++)
			{
        //Deserializzazione del numero identificativo di un pacchetto. 
				packetNumber = (packetsLost[count] << 24);
				packetNumber = packetNumber + (packetsLost[count+1] << 16);
				packetNumber = packetNumber + (packetsLost[count+2] << 8);
				packetNumber = packetNumber + (packetsLost[count+3]);
				//cout<<"Packet number: "<<packetNumber<<endl;
				count = count + 4;
				//Invio al client del pacchetto avente il numero identificativo.
				if(send(s,&packet[packetNumber-1].data[0],(packet[packetNumber-1].payloadDimension+8)*sizeof(char),0) < 0)
					printErrorLog("Packet not sent correctly");
				//sleep(1);
			}
		}

		double s_time = ((double)getTickCount() - det_time)/getTickFrequency();
		cout<<"Sending time (including recovering the broadcast image): T+"<<s_time*1000<<"ms"<<endl;
		//Processing camera image
		detector->detect(cameraImage,distributedKp);
		descriptorExtractor->compute(cameraImage,distributedKp,distributedDescriptors);
		double p_time = ((double)getTickCount() - det_time)/getTickFrequency();
    //Timer stopped
		cout<<"Camera server processing time: T+"<<p_time*1000<<"ms"<<endl;
      
		nodeList[0].time = p_time*1000;
		cout<<"Keypoints found: "<<distributedKp.size()<<endl;
		nodeList[0].keypointsDetected = distributedKp.size();
		for(i=0;i<nodeList[0].keypointsDetected;i++)
		{
			nodeList[0].keypoints.push_back(distributedKp[i].pt.x);
			nodeList[0].keypoints.push_back(distributedKp[i].pt.y);
		}
		//copying distributed descriptors
		nodeList[0].descriptorsSize = nodeList[0].keypointsDetected*64;
		for(i=0;i<nodeList[0].descriptorsSize;i++)
			nodeList[0].descriptors.push_back(distributedDescriptors.data[i]);
		//close(sockfd);

		//RECEIVING DATA FROM THE CLIENTS

		//Waiting for connections. All the clients have to connect to the server, before starting the
		//network test performance.

		double time = receiveData(clientsNumber,nodeList);
		fprintf(file,"%f ",time);
		close(sockfd);
		/*for(int i=0;i<kp.size();i++)
		{
			printf("angle: %f x: %f y: %f size: %f\n",kp[i].angle,kp[i].pt.x,kp[i].pt.y,kp[i].size);
		}*/
		/*
		int result = check(clientsNumber,nodeList,kp,descriptors);
		if(result==0)
			cout<<"Check correct"<<endl;
		else if(result==1)
			printErrorLog("Keypoints number not corrected");
		else if(result==2)
			printErrorLog("At least one keypoint different");
		close(sockfd);
		*/
	}
	fprintf(file,"\n");
	fprintf(lostpackets,"\n");
	fprintf(filebandwidth,"\n");
	fclose(filebandwidth);
	fclose(file);
	fclose(lostpackets);
}



